// 定义电机引脚
int FRONT_LEFT_IN1 = 16;
int FRONT_LEFT_IN2 = 14;

int FRONT_RIGHT_IN3 = 12;
int FRONT_RIGHT_IN4 = 13;

int REAR_LEFT_IN1 = 5;
int REAR_LEFT_IN2 = 4;

int REAR_RIGHT_IN3 = 2;
int REAR_RIGHT_IN4 = 15;

// 定义蓝牙模块的串口引脚
#define BT_RX 3  //
#define BT_TX 1  //

// 初始化硬件串口
HardwareSerial bluetoothSerial = Serial1; // 使用第二个硬件串口

void setup() {
  // 设置所有引脚为输出模式
  pinMode(FRONT_LEFT_IN1, OUTPUT);
  pinMode(FRONT_LEFT_IN2, OUTPUT);
  pinMode(FRONT_RIGHT_IN3, OUTPUT);
  pinMode(FRONT_RIGHT_IN4, OUTPUT);
  pinMode(REAR_LEFT_IN1, OUTPUT);
  pinMode(REAR_LEFT_IN2, OUTPUT);
  pinMode(REAR_RIGHT_IN3, OUTPUT);
  pinMode(REAR_RIGHT_IN4, OUTPUT);

  // 初始化串口通信
  Serial.begin(115200); // 主串口用于调试
  bluetoothSerial.begin(9600, SERIAL_8N1,SERIAL_FULL, BT_RX, BT_TX); 

  // 进入命令模式并配置 HC-05
  delay(1000); // 给蓝牙模块一点时间启动
  bluetoothSerial.print("AT+ROLE?\r\n");
  bluetoothSerial.print("AT+ROLE=1\r\n");
  bluetoothSerial.print("AT+NAME=ADDD\r\n");
  bluetoothSerial.print("AT+PIN=1234\r\n");
  bluetoothSerial.print("AT+AUTOCON=1\r\n");
  bluetoothSerial.print("AT+SAVE\r\n");
}

void loop() {
  // 检查是否有新的蓝牙数据到达
  if (bluetoothSerial.available()) {
    char command = bluetoothSerial.read();

    switch (command) {
      case 'A':
        FRONT(); break;
      case 'B':
        BEHIND(); break;
      case 'C':
        LEFT(); break;
      case 'D':
        RIGHT(); break;
      case 'E':
        LEFTFRONT(); break;
      case 'G':
        RIGHTFRONT(); break;
      case 'H':
        LEFTBEHIND(); break;
      case 'F':
        RIGHTBEHIND(); break;
      case 'O':
        stopMotors(); break;
      case 'I':
        ROLL1(); break;
      case 'J':
        ROLL2(); break;
      default:
        stopMotors();break; 
    }
  }
}

void FRONT() {
  // 前进
  digitalWrite(FRONT_LEFT_IN1, HIGH);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  digitalWrite(FRONT_RIGHT_IN3, HIGH);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  digitalWrite(REAR_LEFT_IN1, HIGH);
  digitalWrite(REAR_LEFT_IN2, LOW);
  digitalWrite(REAR_RIGHT_IN3, HIGH);
  digitalWrite(REAR_RIGHT_IN4, LOW);
}

void BEHIND() {
  // 后退
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, HIGH);
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, HIGH);
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, HIGH);
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, HIGH);
}

void LEFT() {
  // 左转
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, HIGH);
  digitalWrite(FRONT_RIGHT_IN3, HIGH);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  digitalWrite(REAR_LEFT_IN1, HIGH);
  digitalWrite(REAR_LEFT_IN2, LOW);
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, HIGH);
}

void RIGHT() {
  // 右转
  digitalWrite(FRONT_LEFT_IN1, HIGH);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, HIGH);
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, HIGH);
  digitalWrite(REAR_RIGHT_IN3, HIGH);
  digitalWrite(REAR_RIGHT_IN4, LOW);
}

void LEFTFRONT() {
  // 左前斜
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  digitalWrite(FRONT_RIGHT_IN3, HIGH);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  digitalWrite(REAR_LEFT_IN1, HIGH);
  digitalWrite(REAR_LEFT_IN2, LOW);
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, LOW);
}

void RIGHTFRONT() {
  // 右前斜
  digitalWrite(FRONT_LEFT_IN1, HIGH);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, LOW);
  digitalWrite(REAR_RIGHT_IN3, HIGH);
  digitalWrite(REAR_RIGHT_IN4, LOW);
}

void LEFTBEHIND() {
  // 左后斜
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, HIGH);
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, LOW);
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, HIGH);
}

void RIGHTBEHIND() {
  // 右后斜
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, HIGH);
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, LOW);
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, HIGH);
}

void stopMotors() {
  // 停止所有电机
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, LOW);
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, LOW);
}
void ROLL1(){
  // 前左电机
  digitalWrite(FRONT_LEFT_IN1, HIGH);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, HIGH);
   // 后左电机
  digitalWrite(REAR_LEFT_IN1, HIGH);
  digitalWrite(REAR_LEFT_IN2, LOW);
  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, HIGH);
}
void ROLL2(){
  // 前左电机
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, HIGH);
  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, HIGH);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
   // 后左电机
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, HIGH);
  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, HIGH);
  digitalWrite(REAR_RIGHT_IN4, LOW);
}